#include "ros/ros.h"
#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>

using namespace Eigen;
using namespace std;


/*
class PosVelEstimator
{
private:
	bool inited;
public:
	double x,y,z;
	double vx,vy,vz;
	double prev_x,prev_y,prev_z;
	static const double w = 0.2f;
	static const double error_th = 1.1f;
public:
	PosVelEstimator()
	{
		x=y=z=vx=vy=vz=0;
		prev_x=prev_y=prev_z=0;
		inited = false;
	}
	~PosVelEstimator()
	{
	}
	void Apply(double ob_x, double ob_y, double ob_z,double dt)
	{
		if(dt<1.0/200.0) return;
		if (!inited)
		{
			x = ob_x;
			y = ob_y;
			z = ob_z;
			vx = 0;
			vy = 0;
			vz = 0;

			inited = true;
			return;
		}
        prev_x = x;
        prev_y = y;
        prev_z = z;


		double est_vx = (ob_x - prev_x) / dt;
		double est_vy = (ob_y - prev_y) / dt;
		double est_vz = (ob_z - prev_z) / dt;

        float filtered_vx = est_vx * w + vx * (1 - w);
        float filtered_vy = est_vy * w + vy * (1 - w);
        float filtered_vz = est_vz * w + vz * (1 - w);

        if (abs(vx - filtered_vx) < error_th)
        {
            vx = filtered_vx;
        }
        if (abs(vy - filtered_vy) < error_th)
        {
            vy = filtered_vy;
        }
        if (abs(vz - filtered_vz) < error_th)
        {
            vz = filtered_vz;
        }


        x = ob_x;
        y = ob_y;
        z = ob_z;


	}

};
*/

namespace px4_operator_optitrack_pose_sender
{

ros::Publisher vision_pos_pub;
Affine3d pose_opti_enu;
Affine3d pose_uav_rb;

double send_interval = 0.1;


void printUsage()
{
	printf("print this help: -h  --help \n");
	printf("set vrpn tracker name: _tracker_name:=[tracker_name] \n");
	printf("set mavlink send frequency limit(2hz~1000hz): _send_freq:=[send_freq] \n");
}

}

using namespace px4_operator_optitrack_pose_sender;
void optitrackPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);




int main(int argc, char **argv)
{
	ros::init(argc, argv, "optitrack_pose_sender");
	ros::NodeHandle nh;
	for (int i = 1; i < argc; i++) {
		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
			printUsage();
			return 0;
		}
	}

	string tracker_name = "Dozen_UAV_0";
	ros::param::get("~tracker_name", tracker_name);
	ros::param::del("~tracker_name");

	double send_freq = 40.0;
	ros::param::get("~send_freq", send_freq);
	ros::param::del("~send_freq");

	if(send_freq <=0) send_freq = 40.0;
	else if(send_freq <= 2) send_freq = 2;
	else if(send_freq >= 1000) send_freq = 1000;
	send_interval = 1.0 / send_freq;

	ROS_INFO("Use tracker name: %s", tracker_name.c_str());
	ROS_INFO("Pose sending interval = %lf s, freqency = %lf Hz", send_interval, 1.0/send_interval);
	ROS_WARN("Note: actual pose sending frequency is limited by the server's data broadcasting frequency");

	char full_tracker_topic_name[256];
	sprintf(full_tracker_topic_name,"/vrpn_client_node/%s/pose",tracker_name.c_str());

	ros::Subscriber stdreo_sub = nh.subscribe(full_tracker_topic_name, 10, optitrackPoseCallback);
	vision_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/mocap/pose", 10);


	/*
	while(ros::ok())
	{
	  geometry_msgs::PoseStamped fake_pose;
	  fake_pose.header.stamp = ros::Time::now();
	  fake_pose.pose.position.x = 0;
	  fake_pose.pose.position.y = 0;
	  fake_pose.pose.position.z = 2;
	  fake_pose.pose.orientation.x = 0;
	  fake_pose.pose.orientation.y = 0;
	  fake_pose.pose.orientation.z = -0.707;
	  fake_pose.pose.orientation.w = -0.707;

	  vision_pos_pub.publish(fake_pose);
	  usleep(20*1000);
	  ros::spinOnce();
	}
	*/


	pose_opti_enu = Affine3d::Identity();
	pose_opti_enu.linear()<<1,0,0,0,0,-1,0,1,0;
	//cout<<pose_opti_enu.matrix()<<endl;

	Matrix3d pose_init_uav_enu;
	pose_init_uav_enu<<0,-1,0,1,0,0,0,0,1;

	pose_uav_rb.linear() = pose_opti_enu.linear().inverse() * pose_init_uav_enu;
	pose_uav_rb.translation() = Vector3d::Zero();

	cout<<pose_uav_rb.rotation()<<endl;

	ROS_INFO("Optitrack pose sender started.");
	ROS_WARN("Note: if nothing about sending frequency is shown, please check optitrack/network/tracker_name ...");

	ros::spin();

	return 0;
}



unsigned long long frameSeq = 0;

Affine3d pose_rb_opti;
Affine3d pose_uav_enu;

Eigen::Matrix3d att;
Eigen::Vector3d position;

//PosVelEstimator posvelEstimator;
double timer_start_stamp = 0;
const double freq_count_time = 5.0;
int message_count = 0;
double last_send_timestamp = 0;

void optitrackPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	geometry_msgs::PoseStamped::_pose_type pose = msg->pose;
	geometry_msgs::PoseStamped new_msg = *msg;
/*
	double x = msg->pose.position.x;
	double y = msg->pose.position.z;
	double z = -msg->pose.position.y;
	double dt = 1.0 / 120.0;

	posvelEstimator.Apply(x,y,z, dt);
*/
	new_msg.header = msg->header;
	/*
	new_msg.pose.position.x = posvelEstimator.x;
	new_msg.pose.position.y = posvelEstimator.y;
	new_msg.pose.position.z = posvelEstimator.z;
	new_msg.pose.orientation.x = posvelEstimator.vx;
	new_msg.pose.orientation.y = posvelEstimator.vy;
	new_msg.pose.orientation.z = posvelEstimator.vz;
	new_msg.pose.orientation.w = 1;
	*/

	tf::poseMsgToEigen(msg->pose,pose_rb_opti);
	pose_uav_enu = pose_opti_enu * pose_rb_opti * pose_uav_rb;
	tf::poseEigenToMsg(pose_uav_enu,new_msg.pose);
	new_msg.header.stamp = ros::Time::now();
/*
	ROS_INFO("**msg_pose_orientation**[%8.4lf %8.4lf %8.4lf %8.4lf]***********",
			msg->pose.orientation.w,msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z);
	ROS_INFO("**new_msg_orientation******[%8.4lf %8.4lf %8.4lf %8.4lf]***********",
			new_msg.pose.orientation.w,new_msg.pose.orientation.x,new_msg.pose.orientation.y,new_msg.pose.orientation.z);
*/

	message_count ++;
	double time_current = ros::Time::now().toSec();
	double time_duration = time_current-timer_start_stamp;
	if(time_duration > freq_count_time)
	{
		if(message_count >0)
		{
			double avg_freq = message_count / time_duration;
			ROS_INFO("current freqency: %.4lf Hz", avg_freq);
		}

		timer_start_stamp = time_current;
		message_count = 0;
	}





	if(time_current - last_send_timestamp >= send_interval)
	{
		vision_pos_pub.publish(new_msg);
		//ROS_INFO("[%8.4lf %8.4lf %8.4lf %8.4f %8.4f %8.4f]",posvelEstimator.x, posvelEstimator.y, posvelEstimator.z,
							//posvelEstimator.vx, posvelEstimator.vy, posvelEstimator.vz);

		/*
		ROS_INFO("[%8.4lf %8.4lf %8.4lf]",new_msg.pose.position.x, new_msg.pose.position.y, new_msg.pose.position.z);
		ROS_INFO("[%8.4lf %8.4lf %8.4lf %8.4f]",new_msg.pose.orientation.x, new_msg.pose.orientation.y,
				new_msg.pose.orientation.z,new_msg.pose.orientation.w);
				*/

		last_send_timestamp = time_current;
	}
	frameSeq ++;
}


